/*A program to test basic robot movements in a 2-D plane including 90 degree turns
*Written by Sumit Kumar
*/

#include "fourwheelbase.h"


void Base::moveForward(int flspeed, int frspeed, int blspeed, int brspeed)
{
    movement=1;
    digitalWrite(FLDir1,0);
    digitalWrite(FLDir2,1);
    digitalWrite(FRDir1,0);
    digitalWrite(FRDir2,1);
    digitalWrite(BLDir1,0);
    digitalWrite(BLDir2,1);
    digitalWrite(BRDir1,0);
    digitalWrite(BRDir2,1);
    setPwm(flspeed, frspeed, blspeed, brspeed);

}
void Base::moveForward(int speed)
{
    moveForward(speed, speed, speed, speed);
}

void Base::moveBackward(int flspeed, int frspeed, int blspeed, int brspeed)
{
    movement=2;
    digitalWrite(FLDir1,1);
    digitalWrite(FLDir2,0);
    digitalWrite(FRDir1,1);
    digitalWrite(FRDir2,0);
    digitalWrite(BLDir1,1);
    digitalWrite(BLDir2,0);
    digitalWrite(BRDir1,1);
    digitalWrite(BRDir2,0);
    setPwm(flspeed,frspeed,blspeed,brspeed);
}

void Base::moveBackward(int speed)
{
    moveBackward(speed, speed, speed, speed);
}


void Base::moveRight(int flspeed, int frspeed, int blspeed, int brspeed)
{
    movement=3;
    digitalWrite(FLDir1,0);
    digitalWrite(FLDir2,1);
    digitalWrite(FRDir1,1);
    digitalWrite(FRDir2,0);
    digitalWrite(BLDir1,1);
    digitalWrite(BLDir2,0);
    digitalWrite(BRDir1,0);
    digitalWrite(BRDir2,1);
    setPwm(flspeed,frspeed,blspeed,brspeed);
}

void Base::moveRight(int speed)
{
    moveRight(speed, speed, speed, speed);
}


void Base::moveLeft(int flspeed, int frspeed, int blspeed, int brspeed)
{
    movement=4;
    digitalWrite(FLDir1,1);
    digitalWrite(FLDir2,0);
    digitalWrite(FRDir1,0);
    digitalWrite(FRDir2,1);
    digitalWrite(BLDir1,0);
    digitalWrite(BLDir2,1);
    digitalWrite(BRDir2,1);
    digitalWrite(BRDir2,0);
    setPwm(flspeed,frspeed,blspeed,brspeed);
}

void Base::moveLeft(int speed)
{
    moveLeft(speed, speed,speed, speed);
}

void Base::rotateRight(int FLspeed,int FRspeed, int BLspeed, int BRspeed)
{
    digitalWrite(FLDir1,0);
    digitalWrite(FLDir2,1);
    digitalWrite(FRDir1,1);
    digitalWrite(FRDir2,0);
    digitalWrite(BLDir1,0);
    digitalWrite(BLDir2,1);
    digitalWrite(BRDir1,1);
    digitalWrite(BRDir2,0);
    setPwm(FLspeed,FRspeed,BLspeed,BRspeed);
}

void Base::rotateRight(int speed)
{
    rotateRight(speed,speed,speed,speed);
}

void Base::rotateLeft(int FLspeed, int FRspeed, int BLspeed, int BRspeed)
{
    digitalWrite(FLDir1,1);
    digitalWrite(FLDir2,0);
    digitalWrite(FRDir1,0);
    digitalWrite(FRDir2,1);
    digitalWrite(BLDir1,1);
    digitalWrite(BLDir2,0);
    digitalWrite(BRDir1,0);
    digitalWrite(BRDir2,1);
    setPwm(FLspeed,FRspeed,BLspeed,BRspeed);
}

void Base::rotateLeft(int speed)
{
    rotateLeft( speed, speed, speed, speed);
}

void Base::stop()
{
    digitalWrite(FLDir1,1);
    digitalWrite(FLDir2,1);
    digitalWrite(FRDir1,1);
    digitalWrite(FRDir2,1);
    digitalWrite(BLDir1,1);
    digitalWrite(BLDir2,1);
    digitalWrite(BRDir1,1);
    digitalWrite(BRDir2,1);
    // setPwm(0);

	analogWrite(FLPwm,0);
    analogWrite(FRPwm,0);
    analogWrite(BRPwm,0);
    analogWrite(BLPwm,0);
    movement=0;
}

void Base::setPwm(int flspeed, int frspeed, int blspeed, int brspeed)
{
    //speed=100 implies the wheel willl run at the MeanSpeed
    FLSpeed=(flspeed*FLMeanspeed)/100.0;
    FRSpeed=(frspeed*FRMeanspeed)/100.0;
    BLSpeed=(blspeed*BLMeanspeed)/100.0;
    BRSpeed=(brspeed*BRMeanspeed)/100.0;
    FLSpeed=FLSpeed>255?255:FLSpeed;
    FRSpeed=FRSpeed>255?255:FRSpeed;
    BLSpeed=BLSpeed>255?255:BLSpeed;
    BRSpeed=BRSpeed>255?255:BRSpeed;
    
	Serial.print("   FLspeed= ");Serial.print(FLSpeed);
    Serial.print("   FRspeed= ");Serial.println(FRSpeed);  
	
	switch(MovementVal())
	{
		case 1: if(FLSpeed<0)
				{
					digitalWrite(FLDir1,1);		//Moving Backward when PWM<0
					digitalWrite(FLDir2,0);
					FLSpeed=-1*FLSpeed;
				}
				else
				{
					digitalWrite(FLDir1,0);		//Moving Forward when PWM>0
					digitalWrite(FLDir2,1);
				}
				if(FRSpeed<0)
				{
					digitalWrite(FRDir1,1);		//Moving Backward when PWM<0
					digitalWrite(FRDir2,0);
					FRSpeed=-1*FRSpeed;
				}
				else
				{
					digitalWrite(FRDir1,0);		//Moving Forward when PWM>0
					digitalWrite(FRDir2,1);
				}
				break;
		case 2: 
				if(FLSpeed<0)
				{
					digitalWrite(FLDir1,0);		//Moving Forward when PWM<0
					digitalWrite(FLDir2,1);
					FLSpeed=-1*FLSpeed;
				}
				else
				{
					digitalWrite(FLDir1,1);		//Moving Backward when PWM>0
					digitalWrite(FLDir2,0);
				}
				if(FRSpeed<0)
				{
					digitalWrite(FRDir1,0);		//Moving Forward when PWM<0
					digitalWrite(FRDir2,1);
					FRSpeed=-1*FRSpeed;
				}
				else
				{
					digitalWrite(FRDir1,1);		//Moving Backward when PWM>0
					digitalWrite(FRDir2,0);
				}
				break;
	}
    
    analogWrite(FLPwm,FLSpeed);
    analogWrite(FRPwm,FRSpeed);
    analogWrite(BRPwm,BRSpeed);
    analogWrite(BLPwm,BLSpeed);
}

void Base::setPwm(int speed)
{
    setPwm(speed, speed, speed, speed);
}

void Base::attachMotors(int fldir1,int fldir2,int flpwm,int frdir1,int frdir2,int frpwm,int bldir1,int BLDir2,int blpwm,int brdir1,int brdir2,int brpwm)
{
  Base::FLDir1=fldir1;
  Base::FLDir2=fldir2;
  Base::FLPwm=flpwm;
  Base::FRDir1=frdir1;
  Base::FRDir2=frdir2;
  Base::FRPwm=frpwm;
  Base::BLDir1=bldir1;
  Base::BLDir2=BLDir2;
  Base::BLPwm=blpwm;
  Base::BRDir1=brdir1;
  Base::BRDir2=brdir2;
  Base::BRPwm=brpwm;
  pinMode(FLDir1, OUTPUT );
  pinMode(FLDir2, OUTPUT );
  pinMode(FLPwm, OUTPUT );
  pinMode(FRDir1, OUTPUT );
  pinMode(FRDir2, OUTPUT );
  pinMode(FRPwm, OUTPUT );
  pinMode(BLDir1, OUTPUT );
  pinMode(BLDir2, OUTPUT );
  pinMode(BLPwm, OUTPUT );
  pinMode(BRDir1, OUTPUT );
  pinMode(BRDir2, OUTPUT );
  pinMode(BRPwm, OUTPUT );
}

void Base::setMeanSpeeds(int flmeanspeed,int frmeanspeed,int blmeanspeed,int brmeanspeed)
{
  Base::FLMeanspeed=flmeanspeed;
  Base::FRMeanspeed=frmeanspeed;
  Base::BLMeanspeed=blmeanspeed;
  Base::BRMeanspeed=brmeanspeed;
}

bool Base::isMovingForward()
{
    if (movement==1)
    {
        return true;
    }
    else
    {
        return false;
    }
}
bool Base::isMovingBackward()
{
    if (movement==2)
    {
        return true;
    }
    else
    {
        return false;
    }
}
bool Base::isMovingLeft()
{
    if (movement==4)
    {
        return true;
    }
    else
    {
        return false;
    }
}
bool Base::isMovingRight()
{
    if (movement==3)
    {
        return true;
    }
    else
    {
        return false;
    }
}


int Base::flSpeed()
{
   return FLSpeed*100.0/FLMeanspeed; //Since Speed is entered as percentage, Returning the percentage not the FLSpeed
}

int Base::frSpeed()
{
  return FRSpeed*100.0/FRMeanspeed;
}

int Base::blSpeed()
{
  return BLSpeed*100.0/BLMeanspeed;
}

int Base::brSpeed()
{
  return BRSpeed*100.0/BRMeanspeed;
}

int Base::flMeanSpeed()
{
  return FLMeanspeed;
}

int Base::frMeanSpeed()
{
  return FRMeanspeed;
}

int Base::blMeanSpeed()
{
  return BLMeanspeed;
}

int Base::brMeanSpeed()
{
  return BRMeanspeed;
}
int Base::MovementVal()
{
  return movement;
}
Base::Base()
{
    stop();
    movement=0;
}


